#include "printf.h"
#include "AM.h"
#include "Serial.h"
#include "../shared/NetworkPacket.h"


module HubP
{
	uses interface AMSend;
	uses interface Receive;
	uses interface Packet;
	uses interface AMPacket;
	uses interface Leds;
	
	uses interface Boot;
	uses interface SplitControl as RadioPowerControl;

	uses interface SplitControl as SerialControl;

	uses interface AMSend as UartSend[am_id_t id];
	uses interface Receive as UartReceive[am_id_t id];
	uses interface Packet as UartPacket;
	uses interface AMPacket as UartAMPacket;


	uses interface PrintfFlush;
	uses interface SplitControl as PrintfControl;
}
implementation
{
	uint16_t lastReading = 0;
	message_t buffer;

	enum {
		UART_QUEUE_LEN = 12,
		PACKET_BUFFER_LEN = 10,
		FIRST_NODE_ID = 10,
	};

	message_t  uartQueueBufs[UART_QUEUE_LEN];
	message_t  *uartQueue[UART_QUEUE_LEN];
	uint8_t    uartIn, uartOut;
	bool       uartBusy, uartFull;

	network_packet_t *toSend[PACKET_BUFFER_LEN];
	uint8_t firstEmptySend;

	message_t toSendMessageBuffer;
	
	task void sendPacket();

	void handleSerial(network_packet_t* packet);

	event void Boot.booted()
	{
		uint8_t i;

		for (i = 0; i < UART_QUEUE_LEN; i++)
			uartQueue[i] = &uartQueueBufs[i];
		uartIn = uartOut = 0;
		uartBusy = FALSE;
		uartFull = TRUE;


		call RadioPowerControl.start();
		call PrintfControl.start();
		call SerialControl.start();
	}
	


	event void RadioPowerControl.startDone(error_t err)
	{
		if(err != SUCCESS)
			call RadioPowerControl.start();
	}



	event void PrintfControl.startDone(error_t err) {
		if(err == SUCCESS){
			call Leds.led2On();
/*			printf("Hub started successfully\n");
			printf("network_packet_t size: %u\n", sizeof(network_packet_t));
			call PrintfFlush.flush();*/
		}
		else
			call PrintfControl.start();
	}



	event void SerialControl.startDone(error_t error) {
		if (error == SUCCESS) {
			uartFull = FALSE;
			printf("SerialControl started successfully\n");
		}
	}



	event void SerialControl.stopDone(error_t error)
	{
	}



	event void RadioPowerControl.stopDone(error_t err)
	{
	}
	


	event void AMSend.sendDone(message_t * msg, error_t err)
	{
		if(err != SUCCESS)
			call AMSend.send(1, msg, sizeof(network_packet_t));
	}



	
	event message_t * Receive.receive(message_t * msg, void * payload, uint8_t size)
	{
		am_addr_t from = call AMPacket.source(msg);
		if(sizeof(network_packet_t) == size){
			network_packet_t * packet = (network_packet_t *)payload;
			printf("<t>%u<d>%u<s>%u<p>%u</>\n", packet->type, AM_COMPUTER_ID, from, packet->data);
		} else 
			printf("<t>%u<d>%u<s>%u<p></>\n", BAD_PACKET, AM_COMPUTER_ID, from);
		call PrintfFlush.flush();
		call Leds.led0Toggle();
		
		return msg;
	}





  task void sendPacket(){
    network_packet_t * packet;


    network_packet_t * payload = (network_packet_t *)call Packet.getPayload(&toSendMessageBuffer, NULL);

    atomic {
      --firstEmptySend;
      packet = toSend[firstEmptySend];
    }

    payload->type = packet->type;
    payload->destination = packet->destination;
    payload->source = packet->source;
    payload->data = packet->data;

    if(call AMSend.send(payload->destination, &toSendMessageBuffer, sizeof(network_packet_t)) != SUCCESS){
      handleSerial(payload);
    }
    else {
/*      printf("Sent packet to: %u\n", payload->destination);
      call PrintfFlush.flush();*/
    }
  }



  void handleSerial(network_packet_t* packet){
    bool success = TRUE;

/*    printf("Packet from Serial: %u:%u:%u", packet->destination, packet->type, packet->data);
    printf("\n");
    call PrintfFlush.flush();*/
    call Leds.led1Toggle();
		
    if(packet->destination >= FIRST_NODE_ID){
      atomic {
        if(firstEmptySend < PACKET_BUFFER_LEN){
          toSend[firstEmptySend] = packet;
          ++firstEmptySend;
          post sendPacket();				
        }
        else {
          success = FALSE;
        }
      }
    }
    if(success == FALSE){
      call Leds.led0Toggle();
    }
  }




	event message_t *UartReceive.receive[am_id_t id](
			message_t *msg, void *payload, uint8_t len) {
		
		message_t *ret = msg;
		
/* This statement may be useful for debugging */
	/*	printf("Packet size:%u, should be:%u\n", sizeof(network_packet_t), len);*/
		call Leds.led1Toggle();		
		if(len==sizeof(network_packet_t)){
			handleSerial((network_packet_t *)payload);
		}

		
		return ret;
	}


	  event void UartSend.sendDone[am_id_t id](message_t* msg, error_t error){
	}


	event void PrintfFlush.flushDone(error_t err){

	}

	event void PrintfControl.stopDone(error_t err){

	}

}
